

#ifndef CHARGEPROJECT_SCAN_TO_POINTCLOUD2_TOPNG_1H
#define CHARGEPROJECT_SCAN_TO_POINTCLOUD2_TOPNG_1H

// ros
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>


//ubuntu  20.04
#include <eigen3/Eigen/Dense>
#include <opencv2/core/eigen.hpp>
////#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>



// pcl_ros
#include <pcl_ros/point_cloud.h>    

// pcl
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

//sukai 2022-10-13
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/visualization/cloud_viewer.h>

#include <pcl/search/kdtree.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/moment_of_inertia_estimation.h>

#include "tf/transform_datatypes.h"
#include "tf/transform_listener.h"
#include <geometry_msgs/Pose.h>
#include <signal.h>

#include <boost/bind.hpp>
#include <boost/thread.hpp>

#include <mutex>
#include <vector>
#include <queue>

#include<fstream>
#include<ctime>

#include<nav_msgs/OccupancyGrid.h>
#include "geometry_msgs/Twist.h"
#include <thread>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <contnav_srvs/ScanToPointToPng.h>
#include <std_msgs/String.h>
#include "common.h"
#include "ContnavLogger.h"
#include "std_msgs/Header.h"
using namespace Eigen;
using namespace cv;
using namespace std;
using namespace contnav::log;
/**
 * 基于 scan 图像重定位算法
 * sukai 2022-12-14
 */


struct MatchTemplatestruct

{
    double minVal,maxVal;
    Point minLoc,maxLoc;
    double x_Pixe_robotpose_template;
    double y_Pixe_robotpose_template;
    int dst3w;
    int dst3h;
    double yaw;
    double qz;
    double qw;
    int xishu;
    Mat matrixAffine;
};


class ScanToPointCloud2Converter
{
    typedef pcl::PointXYZ PointT;
    typedef pcl::PointCloud<PointT> PointCloudT;
    tf::TransformListener listener;
    tf::StampedTransform transform;
    PointCloudT::Ptr cloud_msg;


private:
    std::string pcdpath="/home/sukai/workspace/chargeWorkerplc/src/chargeproject/output";
    std::string fileOutPut_;
    ros::NodeHandle node_handle_;
    ros::NodeHandle private_node_;
    ros::Subscriber mapSub;
    ros::Publisher scanToPointToPng_poseStamped_pub;
    PointT invalid_point_;
    std::mutex m_ ;
    std::mutex m_scanToPointToPngrFun_;
    std::mutex m_scanToPointToPngrDynamicFun_;
    std::mutex m_new_ ;

    int a=-1;
public:
    ScanToPointCloud2Converter();
    ~ScanToPointCloud2Converter();



    void filterCloud(
            const pcl::PointCloud<PointT>::Ptr &cloud,
            const pcl::PointCloud<PointT>::Ptr &cloud_filtered,
            const char *filter_name, double limitMin, double limitMax);
    cv::Mat eulerAnglesToRotationMatrix(cv::Vec3f &theta);
    void init();
    bool isExDir(string &filepath);
     std::vector<std::string>  split_data(std::string pattern,std::string str);
    int scale_to_255(int pixel_value, int min, int max);
    void use_pcdTopicture_main(const double &dynamic_turn_angle);

    void callBack_map(nav_msgs::OccupancyGrid msg);
    void rosToGlobal_one(double rosX,double rosY,double fx,double fy,double scene_x,double scene_y,double &x_Pixe,double &y_Pixe);
    void timerSendCmdFun();//const ros::TimerEvent &time_event
    //---------------------------------------
    ros::Publisher  cmdpub;
    // initial_pose
    ros::Publisher initial_pose_pub,ScanToPointToPngResult_publisher_;
    ros::ServiceServer scanToPointToPngServer ;
    double origin_x ;
    double origin_y;
    double origin_z ;
    double origin_qx ;
    double origin_qy ;
    double origin_qz;
    double origin_qw ;
    double resolution_f ;
    double width;
    double height ;
    ros::Timer timer3_;
    tf::StampedTransform transformodom;
    tf::StampedTransform transformbaselink;

    int laser_index_min_=0;
    int  laser_index_max_=720;

   //--------------------------------------------
    int m_max=10240;
    int m_level=contnav::log::ContnavLogger::DEBUG;
    int m_MBSize_=1024;
   vector<MatchTemplatestruct> matchTemplatestructs_;
   int startinitialpose=-1;

    string laserFrameId="/base_scan";
    string image2path="/home/spurs9528/map_1.pgm";
    bool  isshowImage=false;
    bool showrobotposefig = true;
    bool isLocationinitialPosefig = true;

    int turnRepeatNum_=0;
    int turnRepeatNum=0;
    string turnRepeatInitialposeTtype="static";

    double fazhimax=0.62;
    double fazhimin=0.4;
    double fazhiLocation=0.55;

    int    dynamic_reverse = 1;
    double dynamic_angle_=360;
    double dynamic_tolerance=0.2;
    double dynamic_angTora=3.1415/180;
    double dynamic_speed=0.35;

    double rotatAngleStatic = 360;
    double rotatAngleStatic_ = 360;
    double angleStaticxishu=1;
    double  rotatAngleStaticThreadnum;

    //------------------------------------------------------------------------

    bool callback_ScanToPointToPngrFun(contnav_srvs::ScanToPointToPng::Request &request  , contnav_srvs::ScanToPointToPng::Response &response);

    void matchTemplatesFunNew( Mat &dst2,  Mat &image2,const double &x_Pixe_robotpose_template,const double &y_Pixe_robotpose_template,const geometry_msgs::Pose  &pose_mapTbase_link,double &yaw,const double &dynamic_turn_angle);

    bool get_odom_angle(string pfamid, string cfamid,double &roll,double &pitch,double &yaw );
    bool getrobotbaselink(string pfamid, string cfamid,double &roll,double &pitch,double &yaw ,geometry_msgs::Pose &pose,Matrix4d &Matrix4d_Transform);
    void globalToRos_one(double x_Pixe,double y_Pixe,double fx,double fy,double scene_x,double scene_y,double &rosX,double &rosY);
    void getrosToGlobal(double rosX,double rosY,double &x_Pixe,double &y_Pixe);
    void getglobalToRos(double x_Pixe,double y_Pixe,double &rosX,double &rosY);
    void initialPoseWithMatchTemplatestructs();
    void ScanCallbackThread(const double &rotatAngle);
    void ScanCallbackDynamicThread(const double &dynamic_turn_angle);
    void ScanCallbacklocationThread(const double &rotatAngleThread);
    void ScanCallbacklocation(const double &rotatAngleThread);
    void timerinitialPoseWithMatchTemplatestructsFun(const ros::TimerEvent &time_event);


    void scanCallbackThreadNew();
    void matchTemplatesFunNew2( Mat dst2,  Mat &image2,const double &x_Pixe_robotpose_template,const double &y_Pixe_robotpose_template,const geometry_msgs::Pose  &pose_mapTbase_link);


    void scantoimageInitposeNew(
            geometry_msgs::Pose pose_mapTbase_link, const Matrix4d &baselinkTlaser_Transform,
             Mat &image2, const sensor_msgs::LaserScan &laserScanMsg,
            double rotatRadian);



};


#endif // CHARGEPROJECT_SCAN_TO_POINTCLOUD2_TOPNG_1H
